#include "control.h"
#include "adc.h"

#define SPEED_KP  5.5
#define SPEED_KI  0.5
#define SPEED_KD  0

#define TURN_KP  0.3
#define TURN_KI  0
#define TURN_KD  0.8



/*--------------------------------------------*/
#define AD_DividedTimes  8   //AD????????

#ifdef FChanel

/*????AD??????----------------------------*/
int AD_MaxValue[4] = {2500,2500,2500,2500};        //??AD???
int AD_MinValue[4] = {10  ,  10,  10,  30};        //??AD???

/*AD?????????----------------------------*/
u16    AD_MeanValue[4],AD_MovingAverage[4][AD_DividedTimes];//???????
u16    AD_Sum[4]; //?????AD?
float  AD_Uniformization[4]={0,0,0,0};//???????
float  g_AD_OUT[4]={0,0,0,0};//????????


float TurnDeviation=0;//??????
float LastTurnDeviation=0;//???????
extern uint16_t g_ADC1_ConvertedValues[NOFCHANEL];//ADC?????


                                         
float Deviation_Calculation(void)
{
  /****************************************************************************/
  /****************************************************************************/
  float Deviation = 0; //???????
  u8 g,i,j,k;
 
  static u16 AD_Value[4][5],AD_Sum[4];//ad?????
  
  
  float KPsum;
  float KDsum;
  
  float AD_LeftSqrt,AD_RightSqrt;   //???????????
  float AD_LeftSum=0,AD_RightSum=0;
  /****************************************************************************/
  /****************************************************************************/
  
  /*****????ADC??????*****/
  for(i=0;i < 5;i++)
  {
    AD_Value[0][i]=g_ADC1_ConvertedValues[0];
    AD_Value[1][i]=g_ADC1_ConvertedValues[1];
    AD_Value[2][i]=g_ADC1_ConvertedValues[2];
    AD_Value[3][i]=g_ADC1_ConvertedValues[3];
  }
  
  
  /****************************************************************************/
  for(i=0;i<4;i++)//??????
  {
    for(i=0;i<4;j++)  
    {
      for(k=0;k<4-j;k++)
      {
        if(AD_Value[i][k] > AD_Value[i][k+1])
        {
          AD_Value[i][k]^=AD_Value[i][k+1];
					AD_Value[i][k+1]^=AD_Value[i][k];
					AD_Value[i][k]^=AD_Value[i][k+1];
        }
      }
    }
  }
  for(i=0;i<4;i++)//??????????
  {
    AD_Sum[i] = AD_Value[i][1] + AD_Value[i][2] + AD_Value[i][3];       
    AD_MeanValue[i] = AD_Sum[i] / 3;	
		AD_Sum[i]=0;
  }
  
  
  for(i = 0;i < AD_DividedTimes-1;i ++)//??????,???????
  {
    AD_MovingAverage[0][i] = AD_MovingAverage[0][i + 1];
    AD_MovingAverage[1][i] = AD_MovingAverage[1][i + 1];
    AD_MovingAverage[2][i] = AD_MovingAverage[2][i + 1];
    AD_MovingAverage[3][i] = AD_MovingAverage[3][i + 1];
  }
  for(i=0;i<4;i++)
  {
    AD_MovingAverage[i][AD_DividedTimes-1] =  AD_MeanValue[i];//?????????
  }
  for(i = 0;i < AD_DividedTimes;i ++)//?????????
  {
    AD_Sum[0] += AD_MovingAverage[0][i];
    AD_Sum[1] += AD_MovingAverage[1][i];
    AD_Sum[2] += AD_MovingAverage[2][i];
    AD_Sum[3] += AD_MovingAverage[3][i];
  }
  for(i=0;i<4;i++)               
  {
    AD_MeanValue[i] = AD_Sum[i] / AD_DividedTimes;//???
    AD_Sum[i] = 0;   
  }
  
 
  for(g=0;g<4;g++) 
  {
    AD_Uniformization[g] = (float)(AD_MeanValue[g] - AD_MinValue[g])/(float)(AD_MaxValue[g] - AD_MinValue[g]); 
    if(AD_Uniformization[g]<=0.0)  AD_Uniformization[g]=0.001;
    if(AD_Uniformization[g]>1.0)  AD_Uniformization[g]=1.0; 
    
    g_AD_OUT[g] = 100 * AD_Uniformization[g];   //?????100
  }
  
  
  AD_LeftSum = g_AD_OUT[0] + g_AD_OUT[1];
  AD_RightSum= g_AD_OUT[2] + g_AD_OUT[3];
  
  AD_LeftSqrt  = sqrt(AD_LeftSum );
  AD_RightSqrt = sqrt(AD_RightSum);
  
  TurnDeviation = (AD_LeftSqrt - AD_RightSqrt)/(AD_LeftSum + AD_RightSum);
  TurnDeviation = TurnDeviation * 10000;
  
  
  
  KPsum =  TurnDeviation * TURN_KP;
  KDsum = (TurnDeviation - LastTurnDeviation) * TURN_KD;
  
  LastTurnDeviation = TurnDeviation;
  
  Deviation = KPsum + KDsum; 
  
  return Deviation;
}

#endif

#ifdef TChanel

	short g_ADC_S[10];
	short offset=0;
	extern uint16_t g_ADC1_ConvertedValues[NOFCHANEL];
	/*????AD??????----------------------------*/
	int AD_MaxValue[2] = {1500,1500};        //??AD???
	int AD_MinValue[2] = {50,50};        	 //??AD???

	/*AD?????????----------------------------*/
	u16    AD_MeanValue[2],AD_MovingAverage[2][AD_DividedTimes];//???????
	u16    AD_Sum[2]; //?????AD?
	float  AD_Uniformization[2]={0,0};//???????
	float  g_AD_OUT[2]={0,0};//????????


	float TurnDeviation=0;//??????
	float LastTurnDeviation=0;//???????
	
float Deviation_Calculation(void)
{
  u8 g,i,j,k;
 
  static u16 AD_Value[2][5],AD_Sum[2];//ad?????
  
  
  float KPsum;
  float KDsum;
  
  float AD_LeftSqrt,AD_RightSqrt;   //???????????
   
  /*****????ADC??????*****/
  for(i=0;i < 5;i++)
  {
	
//		if(offset>0){
//			if(g_ADC1_ConvertedValues[0]>(uint16_t)offset){
//				AD_Value[0][i]=g_ADC1_ConvertedValues[0]-(uint16_t)offset;
//			}else{
//				AD_Value[0][i]=g_ADC1_ConvertedValues[0];
//			}
//		}else{
//			AD_Value[0][i]=g_ADC1_ConvertedValues[0]+(~(uint16_t)offset);
//		}
		
	AD_Value[0][i]=g_ADC1_ConvertedValues[0];
    AD_Value[1][i]=g_ADC1_ConvertedValues[1];
  }
  
  
  /****************************************************************************/
  for(i=0;i<2;i++)//??????
  {
    for(j=0;j<5;j++)  
    {
      for(k=0;k<5-j;k++)
      {
        if(AD_Value[i][k] > AD_Value[i][k+1])
        {
          AD_Value[i][k]^=AD_Value[i][k+1];
					AD_Value[i][k+1]^=AD_Value[i][k];
					AD_Value[i][k]^=AD_Value[i][k+1];
        }
      }
    }
  }
  for(i=0;i<2;i++)//??????????
  {
    AD_Sum[i] = AD_Value[i][1] + AD_Value[i][2] + AD_Value[i][3];       
    AD_MeanValue[i] = AD_Sum[i] / 3;	
		AD_Sum[i]=0;
  }
  
  
  for(i = 0;i < AD_DividedTimes-1;i ++)//??????,???????
  {
    AD_MovingAverage[0][i] = AD_MovingAverage[0][i + 1];
    AD_MovingAverage[1][i] = AD_MovingAverage[1][i + 1];
  }
  for(i=0;i<2;i++)
  {
    AD_MovingAverage[i][AD_DividedTimes-1] =  AD_MeanValue[i];//?????????
  }
  for(i = 0;i < AD_DividedTimes;i ++)//?????????
  {
    AD_Sum[0] += AD_MovingAverage[0][i];
    AD_Sum[1] += AD_MovingAverage[1][i];
  }
	
  for(i=0;i<2;i++)               
  {
    AD_MeanValue[i] = AD_Sum[i] / AD_DividedTimes;//???
    AD_Sum[i] = 0;   
  }
  
 
  for(g=0;g<2;g++) 
  {
    AD_Uniformization[g] = (float)(AD_MeanValue[g] - AD_MinValue[g])/(float)(AD_MaxValue[g] - AD_MinValue[g]); 
    if(AD_Uniformization[g]<=0.0)  AD_Uniformization[g]=0.001;
    if(AD_Uniformization[g]>1.0)  AD_Uniformization[g]=1.0; 
    
    g_AD_OUT[g] = 100 * AD_Uniformization[g];   //?????100
  }
  
  
  AD_LeftSqrt  = sqrt(g_AD_OUT[0]);
  AD_RightSqrt = sqrt(g_AD_OUT[1]);
  
  TurnDeviation = (AD_LeftSqrt - AD_RightSqrt)/(g_AD_OUT[0] + g_AD_OUT[1]);
  TurnDeviation = TurnDeviation * 800;
  
  
  
  KPsum =  TurnDeviation * TURN_KP;
  KDsum = (TurnDeviation - LastTurnDeviation) * TURN_KD;
  
  LastTurnDeviation = TurnDeviation;
  
	
  return (KPsum+KDsum);
}


void CalOffset(){
	
	for(int i=0;i<10;i++){
		g_ADC_S[i]=g_ADC1_ConvertedValues[0]-g_ADC1_ConvertedValues[1];
	}
	
	for(int j=0;j<10;j++){
      for(int k=0;k<10-j;k++)
      {
        if(g_ADC_S[k] > g_ADC_S[k])
        {
          g_ADC_S[k]^=g_ADC_S[k+1];
					g_ADC_S[k+1]^=g_ADC_S[k];
					g_ADC_S[k]^=g_ADC_S[k+1];
        }
      }
   }
	
	 int sum =0;
	 
	 for(int i=2;i<8;i++){
		 sum+=g_ADC_S[i];
	 }
	 
	 offset = sum/6;
}

#endif


